# _*_ coding: utf-8 _*_
# @Author   : Wei Yue
# @Time     : 2023-05-16 16:39
# @Function : 电机模型
import pandas
import pandas as pd
import numpy as np
from utils.MyInterpolate import MyInterpolate2D
from utils.robustDiff import RobustDiff
from scipy.interpolate import interp1d
from motorCalibration.motorContanst import *


class MotorModelCls:
    # IdMap,IqMap,LdMap、LqMap、KeMap
    IdMap = None  # spd,trq -> id
    IdIntep = None
    IqMap = None  # spd,trq -> iq
    IqIntep = None
    LdMap = None  # id,iq -> Ld
    LdIntep = None
    LqMap = None  # id,iq -> Lq
    LqIntep = None
    KeMap = None  # iq ->Ke
    KeIntep = None
    fluxdIntep = None  # id,iq ->fluxd
    fluxqIntep = None  # id,iq ->fluxq
    udMap = None
    uqMap = None
    udIntep = None  # spd,trq ->fluxd
    uqIntep = None  # spd,trq ->fluxq
    path = None  # 电机参数的path
    outCharacterMap = None
    # 逆变器效率
    invMap = None
    # 电机参数(全局变量)
    Rs = 0  # 电阻
    P = 0  # 极对数
    Ud = 0
    Uq = 0
    Id_pre = 0
    Iq_pre = 0
    fluxd_pre = 0
    fluxq_pre = 0
    theta = 0
    we = 0
    Ts = 0  # 周期
    Timer = 0  # 计时器

    def __init__(self, path, Rs, P, Ts):

        self.Rs = Rs
        self.P = P
        self.Ts = Ts
        self.path = path
        self.IdMap = pd.read_excel(path, sheet_name='Id', index_col=0)
        self.IdIntep = MyInterpolate2D.createByDf(self.IdMap)
        self.IqMap = pd.read_excel(path, sheet_name='Iq', index_col=0)
        self.IqIntep = MyInterpolate2D.createByDf(self.IqMap)
        # self.LdMap = pd.read_excel(path, sheet_name='LdMap', index_col=0)
        # self.LdIntep = MyInterpolate2D.createByDf(self.LdMap)
        # self.LqMap = pd.read_excel(path, sheet_name='LqMap', index_col=0)
        # self.LqIntep = MyInterpolate2D.createByDf(self.LqMap)
        # self.KeMap = pd.read_excel(path, sheet_name='KeMap')
        self.outCharacterMap = pd.read_excel(path, sheet_name='out_character', index_col=0)
        self.udMap = pd.read_excel(path, sheet_name='Ud', index_col=0)
        self.udIntep = MyInterpolate2D.createByDf(self.udMap)
        self.uqMap = pd.read_excel(path, sheet_name='Uq', index_col=0)
        self.uqIntep = MyInterpolate2D.createByDf(self.uqMap)
        # iq = self.KeMap.loc[:, 'iq'].values
        # ke = self.KeMap.loc[:, 'ke'].values
        # self.KeIntep = interp1d(iq, ke)
        # self.rdId = RobustDiff(self.Ts, 1000)
        # self.rdIq = RobustDiff(self.Ts, 1000)

    def loadInvEffMap(self, path):
        self.invMap = pd.read_excel(path, index_col=0)

    # 输入路谱文档 输出考虑逆变器效率后的交流数据
    def loadCruiseProfile(self, path):
        if type(self.invMap) is not pd.DataFrame:
            raise Exception('请先载入逆变器效率map')
        cruiseMap = pd.read_excel(path)
        spd = cruiseMap.loc[:, 'SPEED']
        trq = cruiseMap.loc[:, 'TORQUE']
        outDf = pandas.DataFrame(columns=['ACpwr', 'U_AC', 'I_AC', 'cos(phi)'])
        for i in range(len(spd)):
            self.calculateACByUdUq(spd.loc[i], trq.loc[i], outDf)

        # outDf.to_excel('./toexcel.xlsx')
        return outDf

    def calculateAcMatrix(self, spd, trq):

        spd = spd.abs()
        trq = trq.abs()
        size = len(spd)

        calPd = pandas.DataFrame()
        calPd['Time'] = np.arange(0, self.Ts * size, self.Ts)
        calPd['Spd'] = spd
        calPd['We'] = spd2we(spd, self.P)
        calPd['Trq'] = trq
        theta_pre = 0
        for i in range(size):
            theta = theta_pre + calPd.loc[i, 'We'] * self.Ts
            if theta > (2 * pi):
                theta = theta % (2 * pi)
            calPd.loc[i, 'Theta'] = theta
            theta_pre = theta
        Id_next = self.IdIntep.interpArr(calPd['Trq'].values, calPd['Spd'].values)
        print(Id_next)
        print(calPd)
        # calPd.to_excel('./sss.xls')

    def judgeOuterCharcter(self, spd, trq):
        maxTrq = self.outCharacterMap.iloc[math.ceil(abs(spd) / 500)].values[0]
        if trq >= 0:
            if trq < maxTrq:
                return trq
            else:
                return maxTrq
        else:
            if abs(trq) < maxTrq:
                return trq
            else:
                return -maxTrq

    def calculateACByUdUq(self, spd, trq, outDf):
        # 外特性曲线做限制
        trq = self.judgeOuterCharcter(spd, trq)
        self.we = spd2we(spd, self.P)
        self.theta = self.theta + self.we * self.Ts
        if self.theta >= 2 * pi:
            self.theta %= (2 * pi)
        Id_next = self.IdIntep.interp(abs(trq), abs(spd))
        Iq_next = self.IqIntep.interp(abs(trq), abs(spd))
        self.Ud = self.udIntep.interp(abs(trq), abs(spd))
        self.Uq = self.uqIntep.interp(abs(trq), abs(spd))
        # 更新id iq
        Is = math.sqrt(Id_next * Id_next + Iq_next * Iq_next)
        # 输出交流量
        ia, ib, ic = dq2abc(Id_next, Iq_next, self.theta)
        self.Timer += self.Ts
        # plt.scatter(self.Timer,ia,c='y',s=2)
        # plt.scatter(self.Timer,ib,c='g',s=2)
        # plt.scatter(self.Timer,ic,c='r',s=2)
        Us = math.sqrt(self.Ud * self.Ud + self.Uq * self.Uq)
        ua, ub, uc = dq2abc(self.Ud, self.Uq, self.theta)
        if Us == 0 or Is == 0:
            phi = pi / 2
        else:
            phi = math.asin(ua / Us) - math.asin(ia / Is)
        # 正负功率转换
        if (spd >= 0 and trq >= 0) or (spd <= 0 and trq <= 0):
            # 输出功率
            S = (self.Ud * Id_next + self.Uq * Iq_next) * 1.5 / 1000
        else:
            S = -(self.Ud * Id_next + self.Uq * Iq_next) * 1.5 / 1000

        index = len(outDf)
        if S == 0:
            outDf.loc[index, 'ACpwr'] = 0
            outDf.loc[index, 'cos(phi)'] = 0
        else:
            outDf.loc[index, 'ACpwr'] = math.cos(phi) * S
            outDf.loc[index, 'cos(phi)'] = math.cos(phi)
        outDf.loc[index, 'U_AC'] = Us / math.sqrt(2)
        outDf.loc[index, 'I_AC'] = Is / math.sqrt(2)

    def calculateAC(self, spd, trq, outDf):
        # 外特性曲线做限制
        trq = self.judgeOuterCharcter(spd, trq)
        self.we = spd2we(spd, self.P)
        self.theta = self.theta + self.we * self.Ts
        if self.theta >= 2 * pi:
            self.theta %= (2 * pi)
        Id_next = self.IdIntep.interp(abs(trq), abs(spd))
        Iq_next = self.IqIntep.interp(abs(trq), abs(spd))
        Ke_next = self.KeIntep(Iq_next)
        Ld_next = self.LdIntep.interp(Id_next, Iq_next)
        Lq_next = self.LqIntep.interp(Id_next, Iq_next)
        self.Ud = self.Rs * Id_next + Ld_next * (Id_next - self.Id_pre) / self.Ts - self.we * Lq_next * Iq_next
        self.Uq = self.Rs * Iq_next + Lq_next * (Iq_next - self.Iq_pre) / self.Ts + self.we * (
                Ld_next * Id_next + Ke_next)
        # 更新id iq
        Is = math.sqrt(Id_next * Id_next + Iq_next * Iq_next)
        self.Id_pre = Id_next
        self.Iq_pre = Iq_next
        # 输出交流量
        ia, ib, ic = dq2abc(Id_next, Iq_next, self.theta)
        self.Timer += self.Ts
        # plt.scatter(self.Timer,ia,c='y',s=2)
        # plt.scatter(self.Timer,ib,c='g',s=2)
        # plt.scatter(self.Timer,ic,c='r',s=2)
        Us = math.sqrt(self.Ud * self.Ud + self.Uq * self.Uq)
        ua, ub, uc = dq2abc(self.Ud, self.Uq, self.theta)
        if Us == 0 or Is == 0:
            phi = pi / 2
        else:
            phi = math.asin(ua / Us) - math.asin(ia / Is)
        # 正负功率转换
        if (spd >= 0 and trq >= 0) or (spd <= 0 and trq <= 0):
            # 输出功率
            S = (self.Ud * Id_next + self.Uq * Iq_next) * 1.5 / 1000
        else:
            S = -(self.Ud * Id_next + self.Uq * Iq_next) * 1.5 / 1000

        index = len(outDf)
        if S == 0:
            outDf.loc[index, 'ACpwr'] = 0
            outDf.loc[index, 'cos(phi)'] = 0
        else:
            outDf.loc[index, 'ACpwr'] = math.cos(phi) * S
            outDf.loc[index, 'cos(phi)'] = math.cos(phi)
        outDf.loc[index, 'U_AC'] = Us / math.sqrt(2)
        outDf.loc[index, 'I_AC'] = Is / math.sqrt(2)


def testIntep():
    HP26 = MotorModelCls('../excel/motorMap.xlsx', 0.00885, 3, 0.1)
    HP26.loadInvEffMap('./excel/INVmap.xlsx')
    print(HP26.judgeOuterCharcter(12002, 184.94))
    print(HP26.IdIntep.interp(110, 18500))


def mainProcess():
    HP26 = MotorModelCls('../excel/motorMap.xlsx', 0.00885, 3, 0.1)
    HP26.loadInvEffMap('./excel/INVmap.xlsx')
    HP26.loadCruiseProfile('C:/Users/Lenovo/Desktop/MotorSimulink/Even-road.xls')


if __name__ == '__main__':
    # testIntep()
    mainProcess()
